/* 
 * File:   Module.cpp
 * Author: Piotrek
 * 
 * Created on 24 kwiecień 2010, 22:16
 */
#include "stdafx.h"
#include "Module.h"
#include "Robot.h"

#include <math.h>

/// jednostkowa macierz transformacji
float I[4][4]={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};

const float M_PI = (float)System::Math::PI;

Module::Module(char* filepath): R(0.0f), X(0.0f), Y(0.0f), Z(0.0f)
{
	
    int i, j;
	
    for (i=0;i<4;i++)
    {
        for (j=0;j<4;j++)
        {
            if (i==j)
                T[i][j]=1.0f;
            else
                T[i][j]=0.0f;
        }
    }

#ifdef TRIANGLES
	int num = 128;	// przewidywana ilosc trojkatow, pozwala zoptymalizowac wczytywanie modulow
    triangles.reserve(num);

    readTrianglesFromStl(filepath, 0, 0, 0);
#endif

#ifdef VERTEX_ARRAY
	readArraysFromPly(filepath);
#endif

}

Module::Module(char * filepath, float x0, float y0, float z0, char ax): R(0.0f), X(x0), Y(y0), Z(z0)
{
    axis=ax;

    int i, j;
    for (i=0;i<4;i++)
    {
        for (j=0;j<4;j++)
        {
            if (i==j)
                T[i][j]=1.0f;
            else
                T[i][j]=0.0f;
        }
    }

#ifdef TRIANGLES
	int num = 128;	// przewidywana ilosc trojkatow, pozwala zoptymalizowac wczytywanie modulow
    triangles.reserve(num);

    readTrianglesFromStl(filepath, 0, 0, 0);
#endif

#ifdef VERTEX_ARRAY
	readArraysFromPly(filepath);
#endif

}

int Module::addTriangles(char * filepath, float tx, float ty, float tz)
{
    return readTrianglesFromStl(filepath, tx, ty, tz);
}

int Module::readTrianglesFromStl(char * filepath, float tx, float ty, float tz)
{
    char * solid = "solid ascii";
    char * endsolid = "endsolid";

    char * facet = "facet normal";
    char * endfacet = "endfacet";

    char * loop = "outer loop";
    char * endloop = "endloop";

    char * vertex_str = "vertex";

    char buf[80];

    FILE * f = fopen(filepath, "r");

    if (!f)
    {
        printf("Blad: nie istnieje plik %s\n",filepath);
        return -1;
    }

    float n[3], x[3], y[3], z[3];

    // count = 0;

    int vertexCount=0;
    int trianglesCount=0;

    // parsowanie pliku stl line-by-line
    while (fgets(buf, 80, f))
    {
        // wskaznik roboczy
        char *bufWork = buf;

        // usuniecie poprzedzajacych spacji
        while (*(bufWork++) == ' ');
        bufWork--;

        // sprawdzanie "sygnatury" linii
        if (!strncmp(bufWork, vertex_str, strlen(vertex_str)))
        {

            if (vertexCount>2)
                printf("Blad: Vertex Count!\n"); fflush(stdout);

            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

            sscanf(bufWork, "%e %e %e", &x[vertexCount], &y[vertexCount], &z[vertexCount]);
            x[vertexCount]+=tx; y[vertexCount]+=ty; z[vertexCount]+=tz;
            //printf("vertex: %e %e %e\n", x[vertexCount], y[vertexCount], z[vertexCount]);

            vertexCount++;

        } else if (!strncmp(bufWork, solid, strlen(solid)))
            ;//puts("START");
        else if (!strncmp(bufWork, endsolid, strlen(endsolid)))
            ;//puts("STOP");
        else if (!strncmp(bufWork, facet, strlen(facet)))
        {
            vertexCount=0;
            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

            sscanf(bufWork, "%e %e %e", &n[0], &n[1], &n[2]);
        //    printf("normal: %e %e %e\n", n[0], n[1], n[2]);
        } else if (!strncmp(bufWork, endfacet, strlen(endfacet)))
        {
            //puts("EF");
            triangles.push_back(Triangle(n,x,y,z));
            trianglesCount++;
        }
        else if (!strncmp(bufWork, loop, strlen(loop)))
            ;//puts("LOOP");
        else if (!strncmp(bufWork, endloop, strlen(endloop)))
            ;//puts("EndLOOP");
        else
            printf("%s: %s\n",filepath,bufWork);

    }

    fclose(f);
    printf("%s: %i\n",filepath,trianglesCount);
    return trianglesCount;
}

int Module::readArraysFromPly(char * filepath)
{

    char * element_vertex = "element vertex";
    char * element_face = "element face";
	char * end_header = "end_header";

    char buf[80];

    FILE * f = fopen(filepath, "r");

    if (!f)
    {
        printf("Blad: nie istnieje plik %s\n",filepath);
        return -1;
    }
 	
	
    // parsowanie pliku stl line-by-line
    while (fgets(buf, 80, f))
    {
        // wskaznik roboczy
        char *bufWork = buf;

        // usuniecie poprzedzajacych spacji
        while (*(bufWork++) == ' ');
        bufWork--;

		if (!strncmp(bufWork, element_vertex, strlen(element_vertex)))
		{
			// usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 1;
			
			sscanf(bufWork, "%i", &vertexCount);			
		}
		else if (!strncmp(bufWork, element_face, strlen(element_face)))
		{
			// usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 1;
			
			sscanf(bufWork, "%i", &faceCount);
		}
		else if (!strncmp(bufWork, end_header, strlen(end_header)))
			break;
	}
		
	unsigned int i;			    

	vertices= new float[vertexCount*3];
	normals= new float[vertexCount*3];

	vertices0= new float[vertexCount*3];
	normals0= new float[vertexCount*3];
	
	float * vertexPtr = vertices;
	float * normalPtr = normals;
	
	// vertices
	for (i=0; i<vertexCount; i++)
	{
		fgets(buf, 80, f);
		sscanf(buf, "%f %f %f %f %f %f", vertexPtr, vertexPtr+1, vertexPtr+2, normalPtr, normalPtr+1, normalPtr+2);
		vertexPtr += 3;	
		normalPtr += 3;
	}


	int dump;	
	indices = new int[faceCount*3];	
	int * indexPtr = indices;
	
	// indices
	for (i=0; i<faceCount; i++)
	{
		fgets(buf, 80, f);
		sscanf(buf,"%i %i %i %i", &dump, indexPtr, indexPtr+1, indexPtr+2); 
		indexPtr+=3;
	}


    fclose(f);

	for (i=0; i<vertexCount*3; i++)
	{
		vertices0[i] = vertices[i];
		normals0[i] = normals[i];
	}


    printf("%s: %i/%i\n",filepath,vertexCount,faceCount);
    return faceCount;
}

void Module::drawModule()
{
#ifdef TRIANGLES
    glBegin(GL_TRIANGLES); // Drawing Using Triangles

    //printf("size = %i\n",triangles.size());
    
    unsigned int i;

    for (i=0;i<triangles.size();i++)
    {
        #ifdef LIGHT1
        glColor3f(triangles[i].normal.x, triangles[i].normal.y, triangles[i].normal.z);
        #endif
        glNormal3f(triangles[i].normal.x, triangles[i].normal.y, triangles[i].normal.z);

        glVertex3f(triangles[i].vertex[0].x, triangles[i].vertex[0].y, triangles[i].vertex[0].z);
        glVertex3f(triangles[i].vertex[1].x, triangles[i].vertex[1].y, triangles[i].vertex[1].z);
        glVertex3f(triangles[i].vertex[2].x, triangles[i].vertex[2].y, triangles[i].vertex[2].z);

    }
#endif

#ifdef VERTEX_ARRAY
		glEnableClientState(GL_VERTEX_ARRAY);        
        glEnableClientState(GL_NORMAL_ARRAY);

        glVertexPointer(3, GL_FLOAT, 0, vertices);
        glNormalPointer(GL_FLOAT, 0, normals);     

		glDrawElements(GL_TRIANGLES, faceCount*3, GL_UNSIGNED_INT, indices);

        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_NORMAL_ARRAY);
#endif


    // error checking?
    GLenum error;
    while ((error = glGetError()) != GL_NO_ERROR)
    {
        //    const GLubyte errorStr;
        //    errorStr = gluErrorString (error);
        printf("blad %X!!!", error);
        fflush(stdout);
    }
#ifdef TRIANGLES
    glEnd();
#endif
}


void Module::translate(float x, float y, float z)
{

	unsigned int i;
#ifdef TRIANGLES    
    for (i=0;i<triangles.size();i++)
    {
        int j;
        for (j=0;j<3;j++)
        {
            triangles[i].vertex0[j].x+=x;
            triangles[i].vertex0[j].y+=y;
            triangles[i].vertex0[j].z+=z;
        }
    }
#endif

#ifdef VERTEX_ARRAY
	float * vertexPtr = vertices;
	for (i=0; i<vertexCount; i++)
	{
		*vertexPtr += x;
		vertexPtr++;
		*vertexPtr += y;
		vertexPtr++;
		*vertexPtr += z;
		vertexPtr++;
	}	
#endif

}


void Module::addTriangles(Module * module)
{
    unsigned int i;
    for (i=0;i<module->triangles.size();i++)
    {
        this->triangles.push_back(module->triangles[i]);
    }
}

void Module::transform(float Tprev[4][4])
{
    transform(this->X, this->Y, this->Z, this->axis, this->R, Tprev, 1);
}

void Module::transform(float X, float Y, float Z, float axis, float R, float Tprev[4][4], int whichTriangles)
{
    unsigned int i;
    float sinVal, cosVal;
    float x0, y0, z0;
	float nx, ny, nz;
	
	sinVal = sin((float)(R * M_PI / 180.0f));
    cosVal = cos((float)(R * M_PI / 180.0f));

    if (axis=='x') // translacja wokol osi X
    {

        T[0][0] = Tprev[0][0];
        T[0][1] = Tprev[0][1] * cosVal + Tprev[0][2] * sinVal;
        T[0][2] = -Tprev[0][1] * sinVal + Tprev[0][2] * cosVal;
        T[0][3] = Tprev[0][0] * X + Tprev[0][1] * Y + Tprev[0][2] * Z + Tprev[0][3];

        T[1][0] = Tprev[1][0];
        T[1][1] = Tprev[1][1] * cosVal + Tprev[1][2] * sinVal;
        T[1][2] = -Tprev[1][1] * sinVal + Tprev[1][2] * cosVal;
        T[1][3] = Tprev[1][0] * X + Tprev[1][1] * Y + Tprev[1][2] * Z + Tprev[1][3];


        T[2][0] = Tprev[2][0];
        T[2][1] = Tprev[2][1] * cosVal + Tprev[2][2] * sinVal;
        T[2][2] = -Tprev[2][1] * sinVal + Tprev[2][2] * cosVal;
        T[2][3] = Tprev[2][0] * X + Tprev[2][1] * Y + Tprev[2][2] * Z + Tprev[2][3];


        T[3][0] = Tprev[3][0];
        T[3][1] = Tprev[3][1] * cosVal + Tprev[3][2] * sinVal;
        T[3][2] = -Tprev[3][1] * sinVal + Tprev[3][2] * cosVal;
        T[3][3] = Tprev[3][0] * X + Tprev[3][1] * Y + Tprev[3][2] * Z + Tprev[3][3];
    }

    else if (axis=='y') // translacja wokol osi Y
    {

        T[0][0] = Tprev[0][0] * cosVal - Tprev[0][2] * sinVal;
        T[0][1] = Tprev[0][1];
        T[0][2] = Tprev[0][0] * sinVal + Tprev[0][2] * cosVal;
        T[0][3] = Tprev[0][0] * X + Tprev[0][1] * Y + Tprev[0][2] * Z + Tprev[0][3];

        T[1][0] = Tprev[1][0] * cosVal - Tprev[1][2] * sinVal;
        T[1][1] = Tprev[1][1];
        T[1][2] = Tprev[1][0] * sinVal + Tprev[1][2] * cosVal;
        T[1][3] = Tprev[1][0] * X + Tprev[1][1] * Y + Tprev[1][2] * Z + Tprev[1][3];

        T[2][0] = Tprev[2][0] * cosVal - Tprev[2][2] * sinVal;
        T[2][1] = Tprev[2][1];
        T[2][2] = Tprev[2][0] * sinVal + Tprev[2][2] * cosVal;
        T[2][3] = Tprev[2][0] * X + Tprev[2][1] * Y + Tprev[2][2] * Z + Tprev[2][3];

        T[3][0] = Tprev[3][0] * cosVal - Tprev[3][2] * sinVal;
        T[3][1] = Tprev[3][1];
        T[3][2] = Tprev[3][0] * sinVal + Tprev[3][2] * cosVal;
        T[3][3] = Tprev[3][0] * X + Tprev[3][1] * Y + Tprev[3][2] * Z + Tprev[3][3];

    }

    else // translacja wokol osi Z
    {

        T[0][0] = Tprev[0][0] * cosVal + Tprev[0][1] * sinVal;
        T[0][1] = -Tprev[0][0] * sinVal + Tprev[0][1] * cosVal;
        T[0][2] = Tprev[0][2];
        T[0][3] = Tprev[0][0] * X + Tprev[0][1] * Y + Tprev[0][2] * Z + Tprev[0][3];

        T[1][0] = Tprev[1][0] * cosVal + Tprev[1][1] * sinVal;
        T[1][1] = -Tprev[1][0] * sinVal + Tprev[1][1] * cosVal;
        T[1][2] = Tprev[1][2];
        T[1][3] = Tprev[1][0] * X + Tprev[1][1] * Y + Tprev[1][2] * Z + Tprev[1][3];

        T[2][0] = Tprev[2][0] * cosVal + Tprev[2][1] * sinVal;
        T[2][1] = -Tprev[2][0] * sinVal + Tprev[2][1] * cosVal;
        T[2][2] = Tprev[2][2];
        T[2][3] = Tprev[2][0] * X + Tprev[2][1] * Y + Tprev[2][2] * Z + Tprev[2][3];

        T[3][0] = Tprev[3][0] * cosVal + Tprev[3][1] * sinVal;
        T[3][1] = -Tprev[3][0] * sinVal + Tprev[3][1] * cosVal;
        T[3][2] = Tprev[3][2];
        T[3][3] = Tprev[3][0] * X + Tprev[3][1] * Y + Tprev[3][2] * Z + Tprev[3][3];
    }

#ifdef TRIANGLES
    for (i = 0; i < triangles.size(); i++)
    {
        int j;
        for (j = 0; j < 3; j++)
        {

            x0 = triangles[i].vertex0[j].x;
            y0 = triangles[i].vertex0[j].y;
            z0 = triangles[i].vertex0[j].z;


            if (whichTriangles==0)
            {
                triangles[i].vertex0[j].x = T[0][0] * x0 + T[0][1] * y0 + T[0][2] * z0 + T[0][3];
                triangles[i].vertex0[j].y = T[1][0] * x0 + T[1][1] * y0 + T[1][2] * z0 + T[1][3];
                triangles[i].vertex0[j].z = T[2][0] * x0 + T[2][1] * y0 + T[2][2] * z0 + T[2][3];
            }
            else
            {
                triangles[i].vertex[j].x = T[0][0] * x0 + T[0][1] * y0 + T[0][2] * z0 + T[0][3];
                triangles[i].vertex[j].y = T[1][0] * x0 + T[1][1] * y0 + T[1][2] * z0 + T[1][3];
                triangles[i].vertex[j].z = T[2][0] * x0 + T[2][1] * y0 + T[2][2] * z0 + T[2][3];
            }
        }

        nx=triangles[i].normal0.x;
        ny=triangles[i].normal0.y;
        nz=triangles[i].normal0.z;

        if (whichTriangles==0)
        {
            triangles[i].normal0.x = T[0][0] * nx + T[0][1] * ny + T[0][2] * nz;
            triangles[i].normal0.y = T[1][0] * nx + T[1][1] * ny + T[1][2] * nz;
            triangles[i].normal0.z = T[2][0] * nx + T[2][1] * ny + T[2][2] * nz;
        }
        else
        {
            triangles[i].normal.x = T[0][0] * nx + T[0][1] * ny + T[0][2] * nz;
            triangles[i].normal.y = T[1][0] * nx + T[1][1] * ny + T[1][2] * nz;
            triangles[i].normal.z = T[2][0] * nx + T[2][1] * ny + T[2][2] * nz;
        }

    }
#endif

#ifdef VERTEX_ARRAY
	float * vertexPtr;
	float * normalPtr;

	if (whichTriangles==0)
	{
		vertexPtr = vertices0;
		normalPtr = normals0;
	}
	else
	{
		vertexPtr = vertices;
		normalPtr = normals;
	}

	float * vertex0Ptr = vertices0;
	float * normal0Ptr = normals0;


	for (i=0; i<vertexCount; i++)
	{
		x0 = *vertex0Ptr++;
        y0 = *vertex0Ptr++;
	    z0 = *vertex0Ptr++;

		nx = *normal0Ptr++;
		ny = *normal0Ptr++;
		nz = *normal0Ptr++;

		*vertexPtr++ = T[0][0] * x0 + T[0][1] * y0 + T[0][2] * z0 + T[0][3];
        *vertexPtr++ = T[1][0] * x0 + T[1][1] * y0 + T[1][2] * z0 + T[1][3];
        *vertexPtr++ = T[2][0] * x0 + T[2][1] * y0 + T[2][2] * z0 + T[2][3];

	}
#endif
}


/**
 * @page matrix-transformations Macierze transformacji
 *
 * Policzone w Matlebie macierze transformacji wzgledem poszczegolnych osi.

@verbatim
>> a*z

ans =

[          Tprev[0][0]*cosVal+Tprev[0][1]*sinVal,         -Tprev[0][0]*sinVal+Tprev[0][1]*cosVal,                      Tprev[0][2], Tprev[0][0]*X+Tprev[0][1]*Y+Tprev[0][2]*Z+Tprev[0][3]]
[          Tprev[1][0]*cosVal+Tprev[1][1]*sinVal,         -Tprev[1][0]*sinVal+Tprev[1][1]*cosVal,                      Tprev[1][2], Tprev[1][0]*X+Tprev[1][1]*Y+Tprev[1][2]*Z+Tprev[1][3]]
[          Tprev[2][0]*cosVal+Tprev[2][1]*sinVal,         -Tprev[2][0]*sinVal+Tprev[2][1]*cosVal,                      Tprev[2][2], Tprev[2][0]*X+Tprev[2][1]*Y+Tprev[2][2]*Z+Tprev[2][3]]
[          Tprev[3][0]*cosVal+Tprev[3][1]*sinVal,         -Tprev[3][0]*sinVal+Tprev[3][1]*cosVal,                      Tprev[3][2], Tprev[3][0]*X+Tprev[3][1]*Y+Tprev[3][2]*Z+Tprev[3][3]]

>> a*y

ans =

[          Tprev[0][0]*cosVal-Tprev[0][2]*sinVal,                      Tprev[0][1],          Tprev[0][0]*sinVal+Tprev[0][2]*cosVal, Tprev[0][0]*X+Tprev[0][1]*Y+Tprev[0][2]*Z+Tprev[0][3]]
[          Tprev[1][0]*cosVal-Tprev[1][2]*sinVal,                      Tprev[1][1],          Tprev[1][0]*sinVal+Tprev[1][2]*cosVal, Tprev[1][0]*X+Tprev[1][1]*Y+Tprev[1][2]*Z+Tprev[1][3]]
[          Tprev[2][0]*cosVal-Tprev[2][2]*sinVal,                      Tprev[2][1],          Tprev[2][0]*sinVal+Tprev[2][2]*cosVal, Tprev[2][0]*X+Tprev[2][1]*Y+Tprev[2][2]*Z+Tprev[2][3]]
[          Tprev[3][0]*cosVal-Tprev[3][2]*sinVal,                      Tprev[3][1],          Tprev[3][0]*sinVal+Tprev[3][2]*cosVal, Tprev[3][0]*X+Tprev[3][1]*Y+Tprev[3][2]*Z+Tprev[3][3]]


>> a*x

ans =

[                      Tprev[0][0],          Tprev[0][1]*cosVal+Tprev[0][2]*sinVal,         -Tprev[0][1]*sinVal+Tprev[0][2]*cosVal, Tprev[0][0]*X+Tprev[0][1]*Y+Tprev[0][2]*Z+Tprev[0][3]]
[                      Tprev[1][0],          Tprev[1][1]*cosVal+Tprev[1][2]*sinVal,         -Tprev[1][1]*sinVal+Tprev[1][2]*cosVal, Tprev[1][0]*X+Tprev[1][1]*Y+Tprev[1][2]*Z+Tprev[1][3]]
[                      Tprev[2][0],          Tprev[2][1]*cosVal+Tprev[2][2]*sinVal,         -Tprev[2][1]*sinVal+Tprev[2][2]*cosVal, Tprev[2][0]*X+Tprev[2][1]*Y+Tprev[2][2]*Z+Tprev[2][3]]
[                      Tprev[3][0],          Tprev[3][1]*cosVal+Tprev[3][2]*sinVal,         -Tprev[3][1]*sinVal+Tprev[3][2]*cosVal, Tprev[3][0]*X+Tprev[3][1]*Y+Tprev[3][2]*Z+Tprev[3][3]]

@endverbatim
 */
